%%Simulates robot around the maze
%Ryszard S, 14/03/2012
%Altered from particle_robot.m by James and Liam.

%M=[0,0;60,0;60,45;45,45;45,59;106,59;106,105;0,105]
%T = [80 80]


%Main Particle Filter method.
function run_robot (M, T)
%r = radius of robot
r=14;
PadM = mapPad(M,r);
COM_CloseNXT all %prepares workspace
h=COM_OpenNXT(); %look for USB devices
COM_SetDefaultNXT(h); %sets default handle
OpenUltrasonic(SENSOR_1) %Prepare Ultrasonic sensor
particles = initialise(PadM);
%initial blank hypothesis for original location pointing north.
plan = [0 0];
orientation = 0;
req_weight = 0.95; %The required weighting of the largest particle to check for termination
dir = 1;
sonar = 1;
roundingAccuracy = 4;
roundingFactor = 1/roundingAccuracy;

%Main loop
while 1
    %execute planned movement and rotation
    plan(2) = NXTRotate(plan(2));
    plan(1) = NXTMoveLimited(plan(1));
    
    %move particles according to robots plan movement
    particles = move_particles(particles, plan);
    
    %display particles and maze (optional: plot best guess)
    displaySimulatedRobotAndParticles(particles(1,:), particles, M);
    
    %Take signal (single initially) and reweight particles according to new
    %evidence.
    %     [theta,distance] = NXTScan();
    if (sonar == 1)
        [theta,distance] = NXTSonarBelt(dir);
        if dir == -1
            dir = 1;
        else
            dir = -1;
        end
        sonar = sonar + 1;
    elseif sonar == 2
        [theta,distance] = NXTScan();
        sonar = 3;
    else
        [theta,distance] = NXTScan();
        sonar = 1;
    end
    
    particle_signals = [];
    for i = 1:length(theta)
        particle_signals = [particle_signals, get_particle_measurements(particles, M, theta(i))];
    end
    %Reweight returns a sorted list of particles, according to weights, biggest first.
    particles = reweight(particles, particle_signals, distance, M);
    
    %Text display of highest weighted particle. i.e. best guess so far.
    disp('Current best guess: ')
    disp(particles(1, :))
    
    %checks if particles are gathered within a threshold radius
    [valid,center] = found(particles)
    %Check if robot lacks threshold weighted particle to determine location
    if is_lost(particles(1, 4));
        disp('lost')
        
        particles = initialise(PadM);
        plan = [0 0];
        %If the biggest particle's weight is greater than the threshhold,
        %and the robot is at the T point, stop looping.
        %catches if robot is at T (or close enough)
    elseif (valid == 1) & (round(roundingFactor*center(1))/roundingFactor == round(roundingFactor*T(1))/roundingFactor) & (round(roundingFactor*center(2))/roundingFactor == round(roundingFactor*T(2))/roundingFactor)
        %             particles(1, 2) > req_weight && (particles(1, 1) == T(1)) && (particles(1, 2) == T(2))
        [theta,distance] = NXTSonarBelt(dir);
        particle_signals = [];
        for i = 1:length(theta)
            particle_signals = [particle_signals, get_particle_measurements(particles, M, theta(i))];
        end
        particles = reweight(particles, particle_signals, distance, M);
        [valid,center] = found(particles)
        if (valid == 1) & (round(roundingFactor*center(1))/roundingFactor == round(roundingFactor*T(1))/roundingFactor) & (round(roundingFactor*center(2))/roundingFactor == round(roundingFactor*T(2))/roundingFactor)
            victory()
            displaySimulatedRobotAndParticles(particles(1,:), particles, M);
            break
        end
        for i = 1:length(particles)
            if inpolygon(particles(i, 1), particles(i, 2), PadM(:,1), PadM(:, 2))
                plan = plan_motion(particles(i,1:2),T, PadM, particles(i,3));
                break
            end
        end
        
        particles = resample(particles, PadM);
        %   If valid == 0 maybe perform drastic action to find out what's going on?
    else
        for i = 1:length(particles)
            if inpolygon(particles(i, 1), particles(i, 2), PadM(:,1), PadM(:, 2))
                plan = plan_motion(particles(i,1:2),T, PadM, particles(i,3));
                break
            end
        end
        
        particles = resample(particles, PadM);
    end
    %Include if alter normalise to handle number of particles instead
    %of 1 as screws up parent weightings.
    %particles = normalise(particles);
    
    %Fallback, as if 0 then breaks everything!
    
    if size(particles, 1) == 0
        disp('FallbackInitialise!')
        particles = initialise(PadM);
        plan = [0 0];
    end
end
end